#include "ftc.h"

const ACTION_PTR_t actions[ACTION_MAX] = {
    [ACTION_T1]          = action_timer1
  , [ACTION_TAKEOFF]     = action_takeoff
  , [ACTION_T2]          = action_timer2
  , [ACTION_LAND]        = action_land
  , [ACTION_RESET]       = action_reset
  , [ACTION_LAND_RESET]  = action_land_reset
  , [ACTION_STOP]        = action_stop
  , [ACTION_RESUM]       = action_resum
  , [ACTION_REINIT]      = action_reinit
};

const TRANSIT_t transition_table[STATE_MAX][EVENT_MAX] = {
    [STATE_INIT]      [EVENT_TASK]    = {.state=STATE_PRESTART, .action=actions[ACTION_T1]}
  , [STATE_INIT]      [EVENT_T1]      = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_INIT]      [EVENT_TOF]     = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_INIT]      [EVENT_T2]      = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_INIT]      [EVENT_LAND]    = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_INIT]      [EVENT_ABORT]   = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_INIT]      [EVENT_STOP]    = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_INIT]      [EVENT_RESUM]   = {.state=STATE_UNKNOWN,  .action=NULL}

  , [STATE_PRESTART]  [EVENT_TASK]    = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_PRESTART]  [EVENT_T1]      = {.state=STATE_TAKEOFF,  .action=actions[ACTION_TAKEOFF]}
  , [STATE_PRESTART]  [EVENT_TOF]     = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_PRESTART]  [EVENT_T2]      = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_PRESTART]  [EVENT_LAND]    = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_PRESTART]  [EVENT_ABORT]   = {.state=STATE_INIT,     .action=actions[ACTION_RESET]}
  , [STATE_PRESTART]  [EVENT_STOP]    = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_PRESTART]  [EVENT_RESUM]   = {.state=STATE_UNKNOWN,  .action=NULL}

  , [STATE_TAKEOFF]   [EVENT_TASK]    = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_TAKEOFF]   [EVENT_T1]      = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_TAKEOFF]   [EVENT_TOF]     = {.state=STATE_HOLD,     .action=actions[ACTION_T2]}
  , [STATE_TAKEOFF]   [EVENT_T2]      = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_TAKEOFF]   [EVENT_LAND]    = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_TAKEOFF]   [EVENT_ABORT]   = {.state=STATE_DESCENT,  .action=actions[ACTION_LAND]}
  , [STATE_TAKEOFF]   [EVENT_STOP]    = {.state=STATE_STOP,     .action=actions[ACTION_STOP]}
  , [STATE_TAKEOFF]   [EVENT_RESUM]   = {.state=STATE_UNKNOWN,  .action=NULL}

  , [STATE_STOP]      [EVENT_TASK]    = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_STOP]      [EVENT_T1]      = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_STOP]      [EVENT_TOF]     = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_STOP]      [EVENT_T2]      = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_STOP]      [EVENT_LAND]    = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_STOP]      [EVENT_ABORT]   = {.state=STATE_DESCENT,  .action=actions[ACTION_LAND]}
  , [STATE_STOP]      [EVENT_STOP]    = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_STOP]      [EVENT_RESUM]   = {.state=STATE_RESUM,    .action=actions[ACTION_RESUM]}

  , [STATE_RESUM]     [EVENT_TASK]    = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_RESUM]     [EVENT_T1]      = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_RESUM]     [EVENT_TOF]     = {.state=STATE_HOLD,     .action=actions[ACTION_T2]}
  , [STATE_RESUM]     [EVENT_T2]      = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_RESUM]     [EVENT_LAND]    = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_RESUM]     [EVENT_ABORT]   = {.state=STATE_DESCENT,  .action=actions[ACTION_LAND]}
  , [STATE_RESUM]     [EVENT_STOP]    = {.state=STATE_STOP,     .action=actions[ACTION_STOP]}
  , [STATE_RESUM]     [EVENT_RESUM]   = {.state=STATE_UNKNOWN,  .action=NULL}

  , [STATE_HOLD]      [EVENT_TASK]    = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_HOLD]      [EVENT_T1]      = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_HOLD]      [EVENT_TOF]     = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_HOLD]      [EVENT_T2]      = {.state=STATE_DESCENT,  .action=actions[ACTION_LAND]}
  , [STATE_HOLD]      [EVENT_LAND]    = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_HOLD]      [EVENT_ABORT]   = {.state=STATE_DESCENT,  .action=actions[ACTION_LAND_RESET]}
  , [STATE_HOLD]      [EVENT_STOP]    = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_HOLD]      [EVENT_RESUM]   = {.state=STATE_UNKNOWN,  .action=NULL}

  , [STATE_DESCENT]   [EVENT_TASK]    = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_DESCENT]   [EVENT_T1]      = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_DESCENT]   [EVENT_TOF]     = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_DESCENT]   [EVENT_T2]      = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_DESCENT]   [EVENT_LAND]    = {.state=STATE_INIT,     .action=actions[ACTION_REINIT]}
  , [STATE_DESCENT]   [EVENT_ABORT]   = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_DESCENT]   [EVENT_STOP]    = {.state=STATE_UNKNOWN,  .action=NULL}
  , [STATE_DESCENT]   [EVENT_RESUM]   = {.state=STATE_UNKNOWN,  .action=NULL}
};

static struct ev_loop* loop;
static SM_TIMER_t action_timer;
static ev_tstamp timeout;
static ev_tstamp last_activity;

void action_timer1(STATE_MACHINE_t* sm){
  printf("Act T1\n");
  
  ABORT_t abort = ABORT_FALSE; 
  bcshm_write(SHM_ABORT, &sm->shm, &abort);

  bcshm_read(SHM_FLIGHT_TASK, &sm->shm, &sm->flight_task);
  sm->ftc_tlm.timer_1 = time_diff(sm->flight_task.start_time);
  sm->ftc_tlm.timer_2 = sm->flight_task.hang_time; 

  ev_init(&action_timer.timer, timer1_cb);
  action_timer.timer.repeat = ACTION_TIMER_PERIOD;

  timeout = sm->ftc_tlm.timer_1;
  last_activity = ev_now (EV_A);
  
  ev_timer_again(loop, &action_timer.timer);
}

/*
 *  Взлет.
 */
void action_takeoff(STATE_MACHINE_t* sm){
  printf("Act takeoff\n");
  
  FCACTION_t fc_action = FCACTION_TAKEOFF; 
  bcshm_write(SHM_FCACTION, &sm->shm, &fc_action);
}

void action_timer2(STATE_MACHINE_t* sm){
  printf("Act T2\n");

  FCACTION_t fc_action = FCACTION_EMPTY; 
  bcshm_write(SHM_FCACTION, &sm->shm, &fc_action);

  ev_init(&action_timer.timer, timer2_cb);
  action_timer.timer.repeat = ACTION_TIMER_PERIOD;

  timeout = sm->ftc_tlm.timer_2;
  last_activity = ev_now (EV_A);
  
  ev_timer_again(loop, &action_timer.timer);
}

void action_land(STATE_MACHINE_t* sm){
  printf("Act land\n");
  
  FCACTION_t fc_action = FCACTION_LAND; 
  bcshm_write(SHM_FCACTION, &sm->shm, &fc_action);

  ABORT_t abort = ABORT_FALSE; 
  bcshm_write(SHM_ABORT, &sm->shm, &abort);
  
  sm->ftc_tlm.timer_1 = 0;
  sm->ftc_tlm.timer_2 = 0;
}

void action_reset(STATE_MACHINE_t* sm){
  printf("Act reset\n");

  ABORT_t abort = ABORT_FALSE; 
  bcshm_write(SHM_ABORT, &sm->shm, &abort);

  ev_timer_stop(loop, &action_timer.timer);

  sm->ftc_tlm.timer_1 = 0;
  sm->ftc_tlm.timer_2 = 0;
}

void action_land_reset(STATE_MACHINE_t* sm){
  action_land(sm);
  action_reset(sm);
}

void action_reinit(STATE_MACHINE_t* sm){
  printf("Act reset FC\n");

  FCACTION_t fc_action = FCACTION_EMPTY; 
  bcshm_write(SHM_FCACTION, &sm->shm, &fc_action);

  ABORT_t abort = ABORT_FALSE; 
  bcshm_write(SHM_ABORT, &sm->shm, &abort);

  sm->ftc_tlm.timer_1 = 0;
  sm->ftc_tlm.timer_2 = 0;
}

void action_stop(STATE_MACHINE_t* sm){
  printf("Act stop\n");
  
  FCACTION_t fc_action = FCACTION_STOP; 
  bcshm_write(SHM_FCACTION, &sm->shm, &fc_action);
}

void action_resum(STATE_MACHINE_t* sm){
  printf("Act resum\n");
  
  FCACTION_t fc_action = FCACTION_RESUM; 
  bcshm_write(SHM_FCACTION, &sm->shm, &fc_action);
}
/*
 * Получение события из внешней памяти
 */
EVENT_t getEvent(STATE_MACHINE_t* sm){
  ABORT_t abort;
  EVENT_t event;

  bcshm_read(SHM_ABORT, &(sm->shm), &abort);
  if(abort == ABORT_TRUE 
    && sm->curr_state != STATE_INIT
    && sm->curr_state != STATE_DESCENT)
    return EVENT_ABORT;
  else{
    bcshm_read(SHM_EVENT, &(sm->shm), &event);
    return event;
  }
}

/*
 * Инициализация автомата
 */
void state_machine_init(STATE_MACHINE_t* sm){
  sm->curr_state = STATE_INIT;
  sm->curr_event = EVENT_UNKNOWN;

  bcshm_open(&sm->shm);
}

/*
 * Основной переход автомата
 */
void stateMachineTransit(STATE_MACHINE_t* sm){
  EVENT_t event = getEvent(sm);
  printf("\t\t\tEVENT: %d\n", (int)event);

  EVENT_t unk_ev = EVENT_UNKNOWN;
  bcshm_write(SHM_EVENT, &sm->shm, &unk_ev);

  if(event == EVENT_UNKNOWN)
    return;

  TRANSIT_t transit = transition_table[sm->curr_state][event];
  if(transit.state == STATE_UNKNOWN){
    return;
  }
  
  sm->curr_event = event;
  sm->curr_state = transit.state;
  sm->ftc_tlm.state_flg = transit.state;
  sm->ftc_tlm.event = event;

  if(transit.action != NULL)
    transit.action(sm);
}

/*
 * Основной цикл автомата
 */
void stateMachineKernel(){
  STATE_MACHINE_t sm;
  SM_TIMER_t main_timer;
  ftc_tlm_init(&sm.ftc_tlm);
  
  //printf("Start state machine...\n");

  state_machine_init(&sm);
  main_timer.sm = &sm;
  action_timer.sm = &sm;

  loop = ev_loop_new(EVFLAG_NOENV | EVBACKEND_EPOLL);
  ev_init(&main_timer.timer, main_timer_cb);
  main_timer.timer.repeat = MAIN_TIMER_PERIOD;
  
  ev_timer_again(loop, &main_timer.timer);
  ev_run(loop, 0); 
}

static void main_timer_cb(struct ev_loop *loop, ev_timer *w, int revents){
  STATE_MACHINE_t* sm = ((SM_TIMER_t*)w)->sm;

  //printf("\tMain timer cb\n");

  //printf("\t\tCurr state: %d\n", sm->curr_state);
  //printf("\t\tCurr event: %d\n", sm->curr_event);
  
  stateMachineTransit(sm);
  bcshm_write(SHM_FTC_TLM, &sm->shm, &sm->ftc_tlm);

  return;
}

static void timer1_cb(struct ev_loop *loop, ev_timer *w, int revents){
  STATE_MACHINE_t* sm = ((SM_TIMER_t*)w)->sm;
  EVENT_t event = EVENT_T1;

  printf("\tTimer1 cb\n");
  
  ev_tstamp after = last_activity - ev_now (EV_A) + timeout;
  if(after < 0){
    bcshm_write(SHM_EVENT, &sm->shm, &event);
    ev_timer_stop(EV_A_ w);
  }else{
    sm->ftc_tlm.timer_1 = after; 
  }

  return;
}

static void timer2_cb(struct ev_loop *loop, ev_timer *w, int revents){
  STATE_MACHINE_t* sm = ((SM_TIMER_t*)w)->sm;
  EVENT_t event = EVENT_T2;

  printf("\tTimer2 cb\n");

  ev_tstamp after = last_activity - ev_now (EV_A) + timeout;
  if(after < 0){
    bcshm_write(SHM_EVENT, &sm->shm, &event);
    ev_timer_stop(EV_A_ w);
  }else{
    sm->ftc_tlm.timer_2 = after; 
  }

  return;
}

int time_diff(int rqr_time){
  time_t sys_time = time(NULL);
  struct tm* loc_sys_time = localtime(&sys_time);

  int cur_time = loc_sys_time->tm_sec 
               + loc_sys_time->tm_min * 60
               + loc_sys_time->tm_hour * 3600;

  return cur_time >= rqr_time ? 0 : rqr_time - cur_time;
}
